Optimal Reciprocal Collision Avoidance Basic Implementation